#include "yuntai.h"
uint16_t zhentou_yuntai=0,shuzuweizhi_yuntai=0;
uint8_t flag=1;//接收标志位
uint8_t yuntai_jiaodu[10];
uint16_t x_jiaodu=0,y_jiaodu=0;
uint8_t yuntai_jiaodu_x[]="#000PRAD!"; //云台x轴角度
uint8_t yuntai_jiaodu_y[]="#001PRAD!"; //云台y轴角度
uint8_t yuntai_sflj[]="{#000PULK!#001PULK!}"; //云台释放扭矩
uint8_t yuntai_stop[]="{#000PDST!#001PDST!}"; //云台停止
PID_TypeDef yuntai_pid_x; //云台x轴PID
PID_TypeDef yuntai_pid_y; //云台y轴PID//串口1
void Yuntai_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    USART_InitTypeDef USART_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
 
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能GPIOA时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //使能USART1时钟

    GPIO_InitStructure.GPIO_Pin =GPIO_Pin_9; //PA2,PA9
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz
    GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化GPIOA

    GPIO_InitStructure.GPIO_Pin =GPIO_Pin_10; //PA3,PA10
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //复用推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz
    GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化GPIOA

    USART_InitStructure.USART_BaudRate = 115200; //波特率115200
    USART_InitStructure.USART_WordLength = USART_WordLength_8b; //8位数据位
    USART_InitStructure.USART_StopBits = USART_StopBits_1; //1个停止位
    USART_InitStructure.USART_Parity = USART_Parity_No; //无奇偶校验位
    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无流控制
    USART_Init(USART1, &USART_InitStructure); //初始化USART1

    USART_Cmd(USART1, ENABLE); //使能USART1

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); //设置NVIC分组0

    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; //USART1中断通道
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //抢占优先级0
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //响应优先级0
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能中断通道
    NVIC_Init(&NVIC_InitStructure); //初始化NVIC

    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); //使能接收中断
}

void yuntai_fuwei(void){
    uint8_t x=shexiangtou_X(); //获取光点x轴
    uint8_t y=shexiangtou_Y(); //获取光点y轴
    uint8_t zx_x=0,zx_y=0; 
    center(&zx_x,&zx_y); //获取中心点坐标
    uint8_t x1=(zx_x-x)*1.0/10,y1=(zx_y-y)*1.0/10;//步长
    uint16_t out_x=0,out_y=0;
    out_x=P_Yuntai(&yuntai_pid_x,x1,dqian_x_jiaodu()); //计算x轴角度
    out_y=P_Yuntai(&yuntai_pid_y,y1,dqian_y_jiaodu()); //计算y轴角度
    Yuntai_sendjiaodu(out_x,out_y); //发送云台角度
//	Yuntai_sendjiaodu(1000,1000);
}

void yuntai_xunxian(void){
    uint8_t x=shexiangtou_X(); //获取光点x轴
    uint8_t y=shexiangtou_Y(); //获取光点y轴
    uint8_t j_x=0,j_y=0; 
    jiao(&j_x,&j_y); //获取角坐标
    uint8_t x1=(j_x-x)*1.0/10,y1=(j_y-y)*1.0/10;//步长
    uint16_t out_x=0,out_y=0;
    out_x=P_Yuntai(&yuntai_pid_x,x1,dqian_x_jiaodu()); //计算x轴角度
    out_y=P_Yuntai(&yuntai_pid_y,y1,dqian_y_jiaodu()); //计算y轴角度
    Yuntai_sendjiaodu(out_x,out_y); //发送云台角度
}

void Yuntai_SendByte(uint8_t data)
{
    USART_SendData(USART1, data); //发送数据
	while (USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET); //等待发送缓冲区空
}

void Yuntai_SendString(uint8_t *str)
{
    while (*str) //循环发送字符串
    {
        Yuntai_SendByte(*str++); //发送数据
    }
}

void Yuntai_sendjiaodu(uint16_t x,uint16_t y) //发送云台角度
{
    
    //Yuntai_SendString(yuntai_stop); //发送停止命令
    uint8_t str[35]; //定义字符串数组
    sprintf(str,"{#000P%04dT0000!#001P%04dT0000!}",x,y); //格式化字符串
    Yuntai_SendString(str); //发送字符串
}

void yuntai_jieshou(uint8_t data){
    if(data=='#'){
        zhentou_yuntai=1;
        shuzuweizhi_yuntai=1;
    }else if(zhentou_yuntai){
		if(shuzuweizhi_yuntai==1&&data!='0'){
		zhentou_yuntai=0;
        shuzuweizhi_yuntai=0;
		} 
        shuzuweizhi_yuntai++;
            yuntai_jiaodu[shuzuweizhi_yuntai-1]=data;
        if(data=='!'){
            if(yuntai_jiaodu[2]=='0'){
                x_jiaodu=0;
                for(int i=0;i<4;i++){
                    x_jiaodu=(yuntai_jiaodu[i+4]-'0')+x_jiaodu*10;
                }
            }else if(yuntai_jiaodu[2]=='1'){
                y_jiaodu=0;
                for(int i=0;i<4;i++){
                    y_jiaodu=(yuntai_jiaodu[i+4]-'0')+y_jiaodu*10;
                }
            }
            zhentou_yuntai=0;
            shuzuweizhi_yuntai=0;
            flag=0; //接收完成标志位置为0
        }
    }
}

uint16_t dqian_x_jiaodu(void){
    Yuntai_SendString(yuntai_jiaodu_x); //发送云台x轴角度
    flag=1; //开始接收
    while(flag); //等待接收完成
    return x_jiaodu;
}
uint16_t dqian_y_jiaodu(void){
    Yuntai_SendString(yuntai_jiaodu_y); //发送云台y轴角度
    flag=1; //接收标
    while(flag); //等待接收完成
    return y_jiaodu;
}

void yuntai_STOP(void){
    Yuntai_SendString(yuntai_stop); //发送停止命令
}

void yuntai_SFLJ(void){
    Yuntai_SendString(yuntai_sflj); //发送释放扭矩命令
}

void USART1_IRQHandler(void) //USART1中断服务函数
{
    if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中断
    {
        uint8_t data = USART_ReceiveData(USART1); //接收数据
        yuntai_jieshou(data); //处理接收到的数据
        USART_ClearITPendingBit(USART1, USART_IT_RXNE); //清除中断标志位
    }
}
